#include "Components/clock.h"
#include "Components/motor.h"
#include "Components/oled_hardware_i2c.h"
#include "Components/uart.h"
#include "ti_msp_dl_config.h"
#include <ti/driverlib/m0p/dl_interrupt.h>
#include "motion.h"
#include "line_patrol.h"
#include "servo.h"

#define TAG "Main"
char _str_all[128] = {0};

int main(void)
{
  SYSCFG_DL_init();
  SysTick_Init();

  uart_init();
  OLED_Init();
  motor_init();

  OLED_ShowString(0, 0, (uint8_t *)"TI MSPM0 TEST", 16);
  OLED_ShowString(0, 2, (uint8_t *)"===Line Patrol===", 16);

  DL_GPIO_clearPins(LEDs_PORT, LEDs_LED_PIN);

  uart0_Log_str(TAG, "Init sucessfully!");

  motion_speed_ctrl(100, 100);
  // motor_set_mode(MOTOR_REAR | MOTOR_LEFT, MOTOR_FORWARD);
  // motor_set_torque(MOTOR_REAR | MOTOR_LEFT, 500);

  int i = 0;

#if 1
  while (true)
  {
    motion_speed_ctrl(i, i);
    i += 1;
    if (i > 1000)
    {
      i = 0;
      mspm0_delay_ms(3000);

      motion_speed_ctrl(i, i);
      mspm0_delay_ms(3000);
    }
    mspm0_delay_ms(1);
  }
#endif

  while (1)
  {
    int d = get_patroler_center_x();

    if (d < 0)
    {
      uart0_Log_str(TAG, "distanse is invalid! discard!");
      continue;
    }

    patrol_turn_pool(d);

    sprintf(_str_all, "cur:%03d", d);
    // uart0_Log_str(TAG, _str_all);
    OLED_ShowString(0, 4, (uint8_t *)_str_all, 16);
  }
}
